#include "pickingray.h"
#include <math.h>
#include <QDebug>

PickingRay::PickingRay()
{
}

// ver http://schabby.de/picking-opengl-ray-tracing/

void PickingRay::picking(float screenX, float screenY, float viewportWidth, float viewportHeight, float viewAngle, float nearClippingDistance)
{
    // look direction
    view = lookAt - eye;
    view.normalize();

    // screenX
    screenHorizontally = QVector3D::crossProduct(view, up);
    screenHorizontally.normalize();

    // screenY
    screenVertically = QVector3D::crossProduct(screenHorizontally, view);
    screenVertically.normalize();

    float radians = viewAngle*M_PI / 180.0;
    float halfHeight = tan(radians/2)*nearClippingDistance;
    float halfScaledAspectRatio = halfHeight*(viewportWidth/viewportHeight);

    screenVertically *= halfHeight;
    screenHorizontally *= halfScaledAspectRatio;

    clickPosInWorld.setX(eye.x());
    clickPosInWorld.setY(eye.y());
    clickPosInWorld.setZ(eye.z());

    clickPosInWorld += view*nearClippingDistance;

    screenX -= viewportWidth/2.0;
    screenY -= viewportHeight/2.0;

    // normalize to 1
    screenX /= viewportWidth/2.0;
    screenY /= viewportHeight/2.0;

    clickPosInWorld.setX(clickPosInWorld.x() + screenHorizontally.x()*screenX + screenVertically.x()*screenY);
    clickPosInWorld.setY(clickPosInWorld.y() + screenHorizontally.y()*screenX + screenVertically.y()*screenY);
    clickPosInWorld.setZ(clickPosInWorld.z() + screenHorizontally.z()*screenX + screenVertically.z()*screenY);

    direction.setX(clickPosInWorld.x());
    direction.setY(clickPosInWorld.y());
    direction.setZ(clickPosInWorld.z());

    direction -= eye;
    //direction.normalize();

    //qDebug() << "clickPosInWorld -> X:" << clickPosInWorld.x() << " Y:" << clickPosInWorld.y() << " Z:" << clickPosInWorld.z();
    //qDebug() << "direction -> X:" << direction.x() << " Y:" << direction.y() << " Z:" << direction.z();
}

Object3D* PickingRay::throwRay(QVector<Object3D*> objectsInScene)
{
    float t;
    bool picked = false;
    QVector3D closerIntersection;
    QVector3D testIntersection;
    Object3D* pickedObject;
    foreach (Object3D* object, objectsInScene) {
        t = object->intersectionWithLine(direction, eye);

        if (t != NULL) {
            testIntersection.setX(eye.x() + t*direction.x());
            testIntersection.setY(eye.y() + t*direction.y());
            testIntersection.setZ(eye.z() + t*direction.z());

            if (!picked){
                picked = true;
                closerIntersection.setX(testIntersection.x());
                closerIntersection.setY(testIntersection.y());
                closerIntersection.setZ(testIntersection.z());
                pickedObject = object;
            } else if ((testIntersection - eye).length() < (closerIntersection - eye).length()) {
                closerIntersection.setX(testIntersection.x());
                closerIntersection.setY(testIntersection.y());
                closerIntersection.setZ(testIntersection.z());
                pickedObject = object;
            }
        }
    }

    if (picked) {
        return  pickedObject;
    } else {
        return NULL;
    }
}

QVector3D PickingRay::intersectionWithXyPlane()
{
    QVector3D intersection;
    float s = -clickPosInWorld.z() / direction.z();
    intersection.setX(clickPosInWorld.x() + direction.x()*s);
    intersection.setY(clickPosInWorld.y() + direction.y()*s);
    intersection.setZ(0);

    return intersection;
}

void PickingRay::setVectors(float eyeX, float eyeY, float eyeZ,
                            float centerX, float centerY, float centerZ, float upX, float upY, float upZ)
{
    eye.setX(eyeX);
    eye.setY(eyeY);
    eye.setZ(eyeZ);

    lookAt.setX(centerX);
    lookAt.setY(centerY);
    lookAt.setZ(centerZ);

    up.setX(upX);
    up.setY(upY);
    up.setZ(upZ);
}

QVector3D PickingRay::getClickPosInWorld()
{
    return clickPosInWorld;
}

QVector3D PickingRay::getDirection()
{
    return direction;
}
